#include "driverPortConfig.h"
#include "tb6612.h"
#include "pwm.h"

/* 
 * TB6612有6个引脚, 可以驱动输出两路电机, 分别提供不同的速度, 各引脚的功能如下:
 * Ain1, Ain2: 控制A路输出的方向, 10表示正转, 01表示反转
 * PWMA：用于控制A路输出的速度
 * Bin1, Bin2: 控制B路输出的方向, 10表示正转, 01表示反转
 * PWMB：用于控制B路输出的速度
 */

/**
 * @brief 传入参数用于初始化TB6612, 如果传入的参数是TB6612_USE_A_B_PATH表示初始化A路和B路输出, 否则表示只初始化A路输出,
*/
void Motor_Init(Tb6612UseType useType)
{
	/* step1: GPIO桥接在APB2, 所以先开启APB2的时钟, 然后初始化AIN1和AIN2的GPIO端口为推挽输出, 再初始化PWMA */
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
	
	GPIO_InitTypeDef GPIO_InitStructure = {
		.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7,
		.GPIO_Speed = GPIO_Speed_50MHz,
		.GPIO_Mode = GPIO_Mode_Out_PP,
	};
	GPIO_Init(GPIOB, &GPIO_InitStructure);

	/* step2: 如果需要初始化B路输出, 则相应地初始化B路的端口 */
	if (useType == TB6612_USE_A_B_PATH) {
		RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);

		GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12 | GPIO_Pin_11;
		GPIO_Init(GPIOA, &GPIO_InitStructure);

		/*GPIO_InitStructure.GPIO_Pin = TB6612_Bin2_Pin;
		GPIO_Init(TB6612_Bin_Port, &GPIO_InitStructure);*/
	}
	PWM_Init();
}

void Motor_SetLeftSpeed(int8_t speed)
{
	if (speed >= 0) {
		GPIO_SetBits(GPIOA, GPIO_Pin_12);
		GPIO_ResetBits(GPIOA, GPIO_Pin_11);
		PWM_SetCompare2(speed);
	} else {
		GPIO_SetBits(GPIOA, GPIO_Pin_12);
		GPIO_ResetBits(GPIOA, GPIO_Pin_11);
		PWM_SetCompare2(-1 * speed);
	}
}

void Motor_SetRightSpeed(int8_t speed)
{
	if (speed >= 0) {
		GPIO_SetBits(GPIOB, GPIO_Pin_6);
		GPIO_ResetBits(GPIOB, GPIO_Pin_7);
		PWM_SetCompare1(speed);
	} else {
		GPIO_ResetBits(GPIOB, GPIO_Pin_6);
		GPIO_SetBits(GPIOB, GPIO_Pin_7);
		PWM_SetCompare1(-1 * speed);
	}
}
